#include "Weld.h"
#include "PrescribedMotion.h"
#include "RigidBodyNode.h"
#include "Mat6x6.h"
#include "ABAData.h"
#include "EOMData.h"

using namespace RSIM;

///////////////////////////////////////////////////////
// Weld Joint
///////////////////////////////////////////////////////

Weld::Weld(const int& NodeIndex):Joint(WeldJoint){	
	this->NodeIndex_ = NodeIndex;
}

///////////////////////////////////////////////////////

Weld* Weld::New(const int& NodeIndex){
	return new Weld(NodeIndex);
}

///////////////////////////////////////////////////////

void Weld::printJointType() const{
	cout<<"Joint Type=WeldJoint\n";
}

///////////////////////////////////////////////////////

 void Weld::updVelocity(	const State& state, 
				const RigidBodyNode *Parent,
				const Rotation& prF_C_F, 
				const double *betak,
				Vect3& w,
				Vect3& v) const {
					    
	const Vect3& pr_w = Parent->data_->getAngularVelocity();
	const Vect3& pr_v = Parent->data_->getLinearVelocity();
	
	// 1. F_C_prF*pr_w is the absolute angular velocity
	// of the parent expressed in the joint's F frame
	// 2. Vect3(0,0,U_[0]) is the angular velocity of this body
	// which is already expressed in the joint's F frame
	double *wptr = w.wPtr();
	FastOps::transpose_mult_mat33_vec3(prF_C_F.Ptr(),pr_w.Ptr(),wptr);
	
	double v_tmp[3];
	
	// omega x r
	FastOps::cross(pr_w.Ptr(),betak,v_tmp);
	
	// v + omega x r
	v_tmp[0] += pr_v(0);
	v_tmp[1] += pr_v(1);
	v_tmp[2] += pr_v(2);
	
	// convert v + omega x r to F basis of this body
	FastOps::transpose_mult_mat33_vec3(prF_C_F.Ptr(), v_tmp, v.wPtr());		
}

///////////////////////////////////////////////////////

void Weld::calcAKT(	const RigidBodyNode *Parent, 
			const RigidBodyNode *Body,
			const int& ChildID,
			const double *betak,
			State& state,
			Vect6& sAkt) const{
	if(Parent){
		// AKT is calculated in F basis of this body
		double *akt_ptr = sAkt.wPtr();
		const Rotation& prFCF = Body->data_->getRotationFCF();		

		// Angular velocity of parent expressed in F of parent
		const double *parent_ang_ptr = Parent->data_->getAngularVelocity().Ptr();
		double wwr[3];
		
		// cross( w^{k-1}, cross(w^{k-1}, \beta^{k-1}) ) {expressed in F of parent}
		FastOps::wxwxr(parent_ang_ptr, betak, wwr);
		
		// cross( w^{k-1} , w^k_k u^k ) {expressed in F of this body}
		FastOps::transpose_mult_mat33_vec3(prFCF.Ptr(), wwr, akt_ptr+3);
	}
}


///////////////////////////////////////////////////////

void Weld::sweepBackwards(const bool& isLeaf, 
			const int& ChildID,
			const Vect6& sAkt,
			const Rotation& FCF,					    
			const EOMData *eom,
			const double *betak,
			RigidBodyNode *Parent,
			ABAData *aba) const{
	if(Parent){
		Mat6x6 *Ik3 = aba->wgetIk3();
		Vect6 *Fk3 = aba->wgetFk3();	
		double sIk3[36], sFk3[6],cfk[6];
		
		// cFk = Fk3 - Ik3*A^{k-1}_t
		this->calc_cFk((Fk3)->Ptr(),Ik3->Ptr(),sAkt.Ptr(),cfk);
	
		FastOps::spatial_basis_shift_mat(FCF.Ptr(), Ik3->Ptr(), sIk3);
		FastOps::spatial_basis_shift_vec(FCF.Ptr(), cfk, sFk3);	

		double Ik3Child[36], Fk3Child[6];
		this->calc_skbeta_x_Ik3_x_tskbeta(sIk3, betak, Ik3Child);
		this->calc_skbeta_x_cFk(sFk3, betak, Fk3Child);

		Parent->data_->wgetABAData()->wgetIk3()->add(Ik3Child);
		Parent->data_->wgetABAData()->wgetFk3()->add(Fk3Child);
	}// If (Parent)
}

///////////////////////////////////////////////////////

void Weld::sweepForward(const RigidBodyNode *Parent,
		const int& ChildID,
		const Vect6& sAkt,
		const Rotation& FCF,
		const ABAData *aba,
		const double *betak,
		EOMData *eom,
		State& state
		){	
		
	const double *ik3_ptr = aba->getIk3()->Ptr();
	const double *fk3_ptr = aba->getFk3()->Ptr();

	Vect6 *Ak1 = eom->wgetAk1();
	double *ak1_ptr = Ak1->wPtr();
	
	if(Parent){
		const double *Parent_Ak1_Ptr = Parent->data_->getEOMData()->getAk1()->Ptr();
		{
			double ak1_tmp[6];
			FastOps::transpose_skbeta_x_V(Parent_Ak1_Ptr, betak, ak1_tmp);
			FastOps::transpose_spatial_basis_shift_vec(FCF.Ptr(),ak1_tmp, ak1_ptr);
		}
		Ak1->add(sAkt.Ptr());		
	} // if(Parent)
	else{		
		// Spatial acceleration of the F frame of root body.
		double ak1_tmp[]={0.0,0.0,0.0,0.0,0.0,0.0};
		Ak1->copy(ak1_tmp);		
	}
	// calculate the spatial constraint force acting on this body.
	this->calc_m_cFk(fk3_ptr, ik3_ptr, ak1_ptr, eom->wgetFkc1()->wPtr());
}

///////////////////////////////////////////////////////